#include "StdAfx.h"
#include <algorithm>
#include <boost/bind.hpp>
#include "KdTree.h"

bool CompareMaxValue(int iAxis, const CGeometryPtr& spGeom1, const CGeometryPtr& spGeom2)
{
	return spGeom1->GetMaxValue(iAxis) < spGeom2->GetMaxValue(iAxis);
}

CKdTree::CKdTreeNode::CKdTreeNode(int iAxis, int iLevel)
: m_iAxis(iAxis)
, m_iLevel(iLevel)
{
	for (int i = 0; i < 3; ++i)
	{
		m_fRangeMin[i] = 10000.f;
		m_fRangeMax[i] = -10000.f;
	}
}

CKdTree::CKdTreeNode::~CKdTreeNode()
{
}

void CKdTree::CKdTreeNode::ComputeBestSplitPosition()
{
	DATA_TYPE fMinCost = numeric_limits<DATA_TYPE>::max();
	DATA_TYPE fMinCostPosition = 0;
	int iMinCostAxis = 0;

	for (int iAxis = 0; iAxis < 3; ++iAxis)
	{
		DATA_TYPE fMinPosition = m_fRangeMin[iAxis];
		DATA_TYPE fMaxPosition = m_fRangeMax[iAxis];

		vector<DATA_TYPE> vEventPositions;
		for (unsigned int i = 0; i < m_vGeometries.size(); ++i)
		{
			DATA_TYPE fMinValue = m_vGeometries[i]->GetMinValue(iAxis);
			DATA_TYPE fMaxValue = m_vGeometries[i]->GetMaxValue(iAxis);
			if (fMinValue > fMinPosition && fMinValue < fMaxPosition)
				vEventPositions.push_back(fMinValue);
			if (fMaxValue > fMinPosition && fMaxValue < fMaxPosition)
				vEventPositions.push_back(fMaxValue);
		}
		sort(vEventPositions.begin(), vEventPositions.end());
		
		int iStep = (int)(vEventPositions.size() / 2);
		int iCandidateIndex = iStep;

		while (iStep >= 1)
		{
			DATA_TYPE fCandidatePosition = vEventPositions[iCandidateIndex];

			DATA_TYPE fWidth = (fMaxPosition - fMinPosition);
			DATA_TYPE fLeftWidth = (fCandidatePosition - fMinPosition) / fWidth;
			DATA_TYPE fRightWidth = (fMaxPosition - fCandidatePosition) / fWidth;

			int iNumSpheresLeft = 0;
			int iNumSpheresRight = 0;
			for (unsigned int i = 0; i < m_vGeometries.size(); ++i)
			{
				if (m_vGeometries[i]->GetMinValue(iAxis) < fCandidatePosition)
					++iNumSpheresLeft;
				if (m_vGeometries[i]->GetMaxValue(iAxis) > fCandidatePosition)
					++iNumSpheresRight;
			}
			/*
			DATA_TYPE fLeftSum = fLeftWidth * iNumSpheresLeft;
			DATA_TYPE fRightSum = fRightWidth * iNumSpheresRight;
			*/
			DATA_TYPE fLengthLeft[3], fLengthRight[3];
			for (int i = 0; i < 3 ; ++i)
			{
				fLengthLeft[i] = m_fRangeMax[i] - m_fRangeMin[i];
				fLengthRight[i] = fLengthLeft[i];
			}
			fLengthLeft[iAxis] = fCandidatePosition - m_fRangeMin[iAxis];
			fLengthRight[iAxis] = m_fRangeMax[iAxis] - fCandidatePosition;
			DATA_TYPE fLeftSum = fLengthLeft[0] * fLengthLeft[1] + fLengthLeft[1] * fLengthLeft[2] + fLengthLeft[2] * fLengthLeft[0];
			DATA_TYPE fRightSum = fLengthRight[0] * fLengthRight[1] + fLengthRight[1] * fLengthRight[2] + fLengthRight[2] * fLengthRight[0];

			DATA_TYPE fTotal = fLeftSum * iNumSpheresLeft + fRightSum * iNumSpheresRight;
			if (fTotal < fMinCost)
			{
				fMinCost = fTotal;
				fMinCostPosition = fCandidatePosition;
				iMinCostAxis = iAxis;
			}

			iStep /= 2;
			if (fLeftSum > fRightSum)
				iCandidateIndex -= iStep;
			else if(fLeftSum < fRightSum)
				iCandidateIndex += iStep;
			else
				break;
		}
	}
	m_fSplitPosition = fMinCostPosition;
	m_iAxis = iMinCostAxis;
}

void CKdTree::CKdTreeNode::Build(const vector<CGeometryPtr>& vGeometries)
{
	m_vGeometries = vGeometries;

	if (m_vGeometries.size() < 2)
		m_iLevel = 0;

	if (m_iLevel == 0)
		return;

	ComputeBestSplitPosition();

	vector<CGeometryPtr> vLeftGeoms;
	vector<CGeometryPtr> vRightGeoms;
	for (vector<CGeometryPtr>::const_iterator it = m_vGeometries.begin(); it != m_vGeometries.end(); ++it)
	{
		const CGeometryPtr& spGeom = *it;
		if (spGeom->GetMinValue(m_iAxis) < m_fSplitPosition)
			vLeftGeoms.push_back(spGeom);
		if (spGeom->GetMaxValue(m_iAxis) > m_fSplitPosition)
			vRightGeoms.push_back(spGeom);
	}

	//int iNextAxis = (m_iAxis + 1) % 3;

	// if all geoms are in one side, ignore this node and try to divide using next axis.
	if (vLeftGeoms.size() == 0)// || vRightGeoms.size() == 0)
	{
		//m_iAxis = iNextAxis;
		m_iLevel = m_iLevel - 1;
		m_fRangeMin[m_iAxis] = m_fSplitPosition;
		Build(vGeometries);
	}
	else if (vRightGeoms.size() == 0)// || vRightGeoms.size() == 0)
	{
		//m_iAxis = iNextAxis;
		m_iLevel = m_iLevel - 1;
		m_fRangeMax[m_iAxis] = m_fSplitPosition;
		Build(vGeometries);
	}
	else
	{
		m_spLeft = CKdTreeNodePtr(new CKdTreeNode(m_iAxis, m_iLevel - 1));
		m_spLeft->m_fRangeMin = m_fRangeMin;
		m_spLeft->m_fRangeMax = m_fRangeMax;
		m_spLeft->m_fRangeMax[m_iAxis] = m_fSplitPosition;
		m_spLeft->Build(vLeftGeoms);

		m_spRight = CKdTreeNodePtr(new CKdTreeNode(m_iAxis, m_iLevel - 1));
		m_spRight->m_fRangeMin = m_fRangeMin;
		m_spRight->m_fRangeMax = m_fRangeMax;
		m_spRight->m_fRangeMin[m_iAxis] = m_fSplitPosition;
		m_spRight->Build(vRightGeoms);
	}
}

bool CKdTree::CKdTreeNode::IsIntersect(const CRay& ray) const
{
	const CVector3D& vOrigin = ray.GetOrigin();
	const CVector3D& vDir = ray.GetDir();
	const CVector3D& vInvDir = ray.GetInvDir();

	float tmin, tmax, tymin, tymax, tzmin, tzmax;
	if (vDir.x >= 0) 
	{
		tmin = (m_fRangeMin.x - vOrigin.x) * vInvDir.x;
		tmax = (m_fRangeMax.x - vOrigin.x) * vInvDir.x;
	}
	else 
	{
		tmin = (m_fRangeMax.x - vOrigin.x) * vInvDir.x;
		tmax = (m_fRangeMin.x - vOrigin.x) * vInvDir.x;
	}
	if (vDir.y >= 0) 
	{
		tymin = (m_fRangeMin.y - vOrigin.y) * vInvDir.y;
		tymax = (m_fRangeMax.y - vOrigin.y) * vInvDir.y;
	}
	else 
	{
		tymin = (m_fRangeMax.y - vOrigin.y) * vInvDir.y;
		tymax = (m_fRangeMin.y - vOrigin.y) * vInvDir.y;
	}
	if ( (tmin > tymax) || (tymin > tmax) )
		return false;

	if (tymin > tmin)
		tmin = tymin;
	if (tymax < tmax)
		tmax = tymax;
	if (vDir.z >= 0) 
	{
		tzmin = (m_fRangeMin.z - vOrigin.z) * vInvDir.z;
		tzmax = (m_fRangeMax.z - vOrigin.z) * vInvDir.z;
	}
	else 
	{
		tzmin = (m_fRangeMax.z - vOrigin.z) * vInvDir.z;
		tzmax = (m_fRangeMin.z - vOrigin.z) * vInvDir.z;
	}
	if ( (tmin > tzmax) || (tzmin > tmax) )
		return false;

	if (tzmin > tmin)
		tmin = tzmin;
	if (tzmax < tmax)
		tmax = tzmax;

	return (tmax > 0.f);
}

void CKdTree::CKdTreeNode::RayIntersection(const CRay& ray, vector<CGeometryPtr>& vGeometriesOut) const
{
	if (!IsIntersect(ray))
		return;

	if (m_iLevel == 0)
	{
		vGeometriesOut.insert(vGeometriesOut.end(), m_vGeometries.begin(), m_vGeometries.end());
		return;
	}

	m_spLeft->RayIntersection(ray, vGeometriesOut);
	m_spRight->RayIntersection(ray, vGeometriesOut);
}

CKdTree::CKdTree(void)
{
}


CKdTree::~CKdTree(void)
{
}

void CKdTree::Build(const vector<CGeometryPtr>& vGeometries)
{
	int iMaxKdtreeLevel = (int)(logf((float)vGeometries.size()) / logf(2.f)) * 2;
	m_spRoot = CKdTreeNodePtr(new CKdTreeNode(0, iMaxKdtreeLevel));

	for (unsigned int i = 0; i < vGeometries.size(); ++i)
	{
		for (unsigned int iAxis = 0; iAxis < 3; ++iAxis)
		{
			DATA_TYPE fMinValue = vGeometries[i]->GetMinValue(iAxis);
			DATA_TYPE fMaxValue = vGeometries[i]->GetMaxValue(iAxis);

			if (fMinValue < m_spRoot->m_fRangeMin[iAxis])
				m_spRoot->m_fRangeMin[iAxis] = fMinValue;
			if (fMaxValue > m_spRoot->m_fRangeMax[iAxis])
				m_spRoot->m_fRangeMax[iAxis] = fMaxValue;
		}
	}

	m_spRoot->Build(vGeometries);
}

bool ComparePointers(const CGeometryPtr& spGeom1, const CGeometryPtr& spGeom2)
{
	return &(*spGeom1) < &(*spGeom2);
}

void CKdTree::RayIntersection(const CRay& ray, vector<CGeometryPtr>& vGeometriesOut) const
{
	vGeometriesOut.clear();
	m_spRoot->RayIntersection(ray, vGeometriesOut);

	sort(vGeometriesOut.begin(), vGeometriesOut.end(), ComparePointers);
	vector<CGeometryPtr>::iterator it = unique(vGeometriesOut.begin(), vGeometriesOut.end());
	vGeometriesOut.erase(it, vGeometriesOut.end());

	/*
	static int stored = 0;
	int diff = vGeometriesOut.size() - m_spRoot->m_vGeometries.size();
	if (diff != stored)
	{
		if (diff < 0)
		{
			cout << "Acceleration : " << m_spRoot->m_vGeometries.size() << " -> " << vGeometriesOut.size() << endl;
		}
		else if (diff > 0)
		{
			cout << "No Accelera : " << m_spRoot->m_vGeometries.size() << " -> " << vGeometriesOut.size() << endl;

		}
		stored = diff;
	}
	*/
	
	
}